%module CoordinateDriveClient

%include std_string.i
//%include std_list.i
//%include std_map.i
%include "typemaps.i"
%include std_vector.i

%{
#include "MaCI.hpp"
#include "CoordinateDriveData.hpp"
#include "CoordinateDriveTypes.hpp"
#include "CoordinateDriveClient.hpp"
#include "thread.hpp"
#include "sync.hpp"
//#include <map>
#include <vector>

%}

%include "CoordinateDriveTypes.i"
%include "CoordinateDriveData.i"
%include "../MaCIError.i"
%include "../MaCICtrl/MaCICtrlClient.i"
%include "../common/timestamp.i"


//%template(TCoordinateArray) std::vector<MaCI::CoordinateDrive::TCoordinate2D>;
//%template(TDriveModeArray) std::vector<MaCI::CoordinateDrive::TDriveMode>;



namespace MaCI {
  namespace CoordinateDrive {


    typedef std::vector<TDriveMode> TDriveModeArray;
    typedef std::vector<TCoordinate2D> TCoordinateArray;



    /** std::vector wrapper for TCoordinate2D elements.
     *
     * This container hides the template parameters of STL classes,
     * allowing Python wrappers to handle the elements more
     * easily. NOTE: In C++ applications, there is no sense in using
     * this container, as all functionality is already built in the
     * STL container class 'std::vector'.
     *
     * \note This could have been easily implemeted as template class
     * to allow using it in other interfaces too, but as the main goal
     * was to hide this behaviour, this class needs to be explicitly
     * defined on per type basis.
     */
    class TCoordinateContainer
    {
    public:
      /** Default, empty constructor.
       *
       */
      TCoordinateContainer(void) 
        : array() {}


      /** Copy from TCoordinate2D vector.
       */
      TCoordinateContainer(const std::vector< MaCI::CoordinateDrive::TCoordinate2D > &aArray) 
        : array(aArray) {}
      
      
      /** Array of elements of type 'TCoordinate2D'.
       */
      std::vector< MaCI::CoordinateDrive::TCoordinate2D > array;
      
      
      /** Get array size in elements.
       *
       * This function returns the number of elements stored in this
       * container.
       *
       * @return                    Number of elements in the container.
       */
      inline const unsigned int GetArraySize(void) const {
        return array.size();
      }
      
      
      /** Return single array entry.
       *
       * This function returns single array entry by index.
       *
       * \note This function returns a constant reference to
       * object. Therefore, this element is only valid if the
       * underlying container (std::vector< TCoordinate2D >) remains
       * unchanged.
       * 
       * @return Constant reference to single array entry.
       *
       */
      inline const TCoordinate2D &GetArrayElement(const unsigned int aIndex) {
        return array[aIndex];
      }


      /** Clear associated array.
       */
      inline void Clear(void) {
        array.clear();
      }
    };


    /** std::vector wrapper for TDriveMode elements.
     *
     * This container hides the template parameters of STL classes,
     * allowing Python wrappers to handle the elements more
     * easily. NOTE: In C++ applications, there is no sense in using
     * this container, as all functionality is already built in the
     * STL container class 'std::vector'.
     *
     * \note This could have been easily implemeted as template class
     * to allow using it in other interfaces too, but as the main goal
     * was to hide this behaviour, this class needs to be explicitly
     * defined on per type basis.
     */
    struct TDriveModeContainer {
    public:
      /** Default, empty constructor.
       *
       */
      TDriveModeContainer(void) 
        : array() {}
      

      /** Copy from TDriveMode vector.
       */
      TDriveModeContainer(const std::vector< MaCI::CoordinateDrive::TDriveMode > &aArray) 
        : array(aArray) {}


      /** Array of elements of type 'TCoordinate2D'.
       */
      std::vector< MaCI::CoordinateDrive::TDriveMode > array;
      
      
      /** Get array size in elements.
       *
       * This function returns the number of elements stored in this
       * container.
       *
       * @return                    Number of elements in the container.
       */
      inline const unsigned int GetArraySize(void) const {
        return array.size();
      }
      
      
      /** Return single array entry.
       *
       * This function returns single array entry by index.
       *
       * \note This function returns a constant reference to
       * object. Therefore, this element is only valid if the
       * underlying container (std::vector< TCoordinate2D >) remains
       * unchanged.
       * 
       * @return Constant reference to single array entry.
       *
       */
      inline const TDriveMode &GetArrayElement(const unsigned int aIndex) {
        return array[aIndex];
      }


      /** Clear associated array.
       */
      inline void Clear(void) {
        array.clear();
      }
    };


    /** MaCI - CoordinateDrive Client interface.
     * 
     * This class contains the methods and types for using
     * a MaCI::CoordinateDrive service in the GIMnet.
     * 
     * 
     */
    class CCoordinateDriveClient : private gim::CSync,
				   private gim::CThread,
                                   public MaCI::CMaCI
    {
    public:

      /** Constructor.
       * 
       * Constructor of the CoordinateDriveClient. This requires;
       * a pointer to Initialized and connected GIMI, minor
       * number of this interface (Used as GIMI minor number)
       *
       * @param[in] aGIMIPtr Pointer to GIMI used for communication
       * @param[in] aInterfaceMinor Interface minor number. Usually
       *            used to indicate the instance of devices of same
       *            type.
       */     
      CCoordinateDriveClient(gimi::GIMI *aGIMIPtr, const int aInterfaceMinor);

      
      /** Destructor.
       */
      ~CCoordinateDriveClient();
      

      /** Open client interface.
       */
      MaCI::EMaCIError DoOpen(void);


      /** Close client interface.
       */
      MaCI::EMaCIError DoClose(void);

      /**
       * Sends a new coordinate to coordinateDrive. 
       * 
       * @param[in] aX                   The x coordinate(in meters)
       * @param[in] aY                   The y coordinate(in meters)
       * @param[in] aA                   The angle of the robot at the end. 
       *                                 NOTE: This is used by coordinateDrivewhen 
       *                                 the mode is KModeUseAngle.
       * @param[in] aIsDifferential      True if the given coordinate is differential,
       *                                 False if it's global
       * @param[in] aIsPathTargetPoint   Define if the coordinate is a path or normal target point. To 
       *                                 drive the path target point the drive mode has to set
       *                                 to KModePathDriving.
       * @param[in] aIsWaitPoint         True if the given coordinate is a point where to wait
       *                                 for continue command, false if it's not. 
       *                                 NOTE: This is used when the coordinateDrive mode
       *                                 is KModePathDriving
       * 
       */
      bool SetCoordinate(float aX = 0.0, 
			 float aY = 0.0, 
			 float aA = 0.0,
			 bool aIsDifferential = false,
			 bool aIsPathTargetPoint = false,
			 bool aIsWaitPoint = false);

			 /**
			 * Sends a new coordinate to coordinateDrive server. 
			 * 
			 * @param aCoordinate              Coordinate to set
			 * @return                         False if command sending failed, true on success
			 */
      bool SetCoordinate(const MaCI::CoordinateDrive::TCoordinate2D &aCoordinate);
	  
			 
		/**
       * Removes all the coordinates, normal AND path coordinates
       * 
       * @return                      False if command sending failed, true on success
       */
      bool RemoveCoordinates();

      /**
       * Starts to drive toward the first coordinate of the list
       *
       * @return                      False if command sending failed, true on success
       */
      bool SetStart();

      /**
       * Stops driving to target
       *
       * @return                      False if command sending failed, true on success
       */
      bool SetStop();

      /**
       * Continue driving when waiting in a wait point.
       *
       * @return                      False if command sending failed, true on success
       */
      bool Continue();

      /**
       * Sets driving mode. Multiple drive modes can on at the same time. 
       * 
       *
       *
       * @return                      False if command sending failed, true on success
       *
       * \see ECoordinateDriveType
       */
      bool SetDriveMode(MaCI::CoordinateDrive::ECoordinateDriveMode aMode);

      /**
       * Unset the wanted driving mode.
       *
       * @return                      False if command sending failed, true on success
       *
       * \see ECoordinateDriveMode
       */
      bool UnsetDriveMode(MaCI::CoordinateDrive::ECoordinateDriveMode aMode);

      /**
       * Get the latest point reached with coordinateDrive
       *
       * @param aCoordinate           TCoordinate2D-struct where can be found information
       *                              about the reached coordinate
       *
       * @param aSeq                  Block until greater sequence number
       *                              available.
       *                              Initially, set the pointed data to 0.
       *                              After that, don't touch the value,
       *                              but pass it on on each call. 
       * @param[in] aTimeout_ms       Timeout int ms. The function block until 
       *                              greater sequence number is available or 
       *                              timeout reached.
       * @return                      True; on success (data stored)
       *                              False; on failure (no data stored
       *                              or partial)
       *
       * \see CCoordinateDriveData
       */
      bool GetLatestPointReached(MaCI::CoordinateDrive::TCoordinate2D &INOUT,
                                 MaCI::Common::TTimestamp &aTimestamp,
				 int *INOUT = NULL,
				 int aTimeout_ms = -1);

      /**
       * Gives all the normal coordinates that hasn't been reached already and
       * all the path coordinates. 
       *
       * @param[out] aArray           The array of coordinates is stored here.
       *                              The array is cleared before filling new information
       * @param aSeq                  Block until greater sequence number
       *                              available.
       *                              Initially, set the pointed data to -1.
       *                              After that, don't touch the value,
       *                              but pass it on on each call. 
       * @param[in] aTimeout_ms       Timeout value. If the operation is not completed
       *                              before timeout is reached, the operation fails.
       * @return                      True if succeeded, false if failed(timeout, no connection..)
       */
      bool GetCoordinates(MaCI::CoordinateDrive::TCoordinateArray &INOUT,
                                 MaCI::Common::TTimestamp &aTimestamp,
                          int *INOUT = NULL,
			  unsigned int aTimeout_ms = -1);



	  bool GetCoordinates(MaCI::CoordinateDrive::TCoordinateContainer &INOUT,
                                 MaCI::Common::TTimestamp &aTimestamp,
                           int *INOUT,
                                unsigned int aTimeout_ms);

    /**
       * Get server status message Event. 
       *
       * Currently possilbe server status are KCommandUnableToDriveToCoordinate,
       * KCommandPathReady and KCommandCalculating.
       *
       * KCommandUnableToDriveToCoordinate contains also the coordinate where the server
       * was unable to drive
       * 
       * KCommandPathReady is for informing that the path is ready to execute. No additional
       *                   data comes with this message.
       *       
       * KCommandCalculating is for informing that the server is calculating the path. No
       *                     additional data comes with this message.
       *
       *
       * NOTE: This is a new feature and is not supported by every server.
       *
       *
       * @param[out] aData            The Data coming from server is stored here.
       *                              The array is cleared before filling new information
       * @param[out] aTimestamp       Timestamp 
       * @param[in] aSeq              Sequence number of the received data. 
       *                              This should increment by one on each call.
       * @param[in] aTimeout_ms       Timeout value. If the operation is not completed
       *                              before timeout is reached, the operation fails.
       * @return                      True if succeeded, false if failed(timeout, no connection..)
       */
      bool GetServerStatusMessage(MaCI::CoordinateDrive::CCoordinateDriveData &aData,
                                  MaCI::Common::TTimestamp &aTimestamp,
                                 int *INOUT,
                                 unsigned int aTimeout_ms);




//      int GetCoordinatesNumber(void);

      /**
       * Gives all the normal coordinates that hasn't been reached already and
       * all the path coordinates. 
       *
       * @param[out] aArray           The struct to store coordinate
       * @param[in] aNumber            The order number of coordinate to return
       * @param[in] aTimeout_ms       Timeout value. If the operation is not completed
       *                              before timeout is reached, the operation fails.
       * @return                      True if succeeded, false if failed(timeout, no connection..)
       */
//      bool GetCoordinate(MaCI::CoordinateDrive::TCoordinate2D &INOUT, int aNumber,
//			  unsigned int aTimeout_ms);

      /**
       * Gives length of the path left. 
       *
       * @param[out] aLinear          Sum of length of line connecting coordinates
       * @param[out] aRot             Sum of turns between orientations in points
       * @return                      True if succeeded, false if failed(timeout, no connection..)
       */
      bool GetPathLength(float *INOUT, float *INOUT);

      /**
       * Gives the drivemodes that is set.
       *
       * @param[out] aArray           The array of driveModes is stored here.
       *                              The array is cleared before filling new information
       * @param[in] aTimeout_ms       Timeout value. If the operation is not completed
       *                              before timeout is reached, the operation fails.
       * @return                      True if succeeded, false if failed(timeout, no connection..)
       */
      bool GetOpenDrivingModes(MaCI::CoordinateDrive::TDriveModeArray &aArray,
			       unsigned int aTimeout_ms);

       bool GetOpenDrivingModes(MaCI::CoordinateDrive::TDriveModeContainer &aArray,
                                      unsigned int aTimeout_ms);
      /**
       * Asks for a plan between 2 coordinates from the server. 
       * \note Don't expect for a good plan from every coordinateDrive-server. Ask only from servers
       *       you know that answers right plans for this query.
       *
       * @param[in] aCoordinateFrom   Coordinate where the plan should begin.
       * @param[in] aCoordinateTo     Coordinate where the plan should end.
       * @param[out] aPlan            Plan is stored here if the server responses. The array is
       *                              cleared before filling new information.
       * @param[in] aTimeout_ms       Timeout value. If the operation is not completed
       *                              before timeout is reached, the operation fails.
       * @return                      True if succeeded, false if failed(timeout, no connection..)
       */      
      bool GetPlan(const MaCI::CoordinateDrive::TCoordinate2D &aCoordinateFrom,
                   const MaCI::CoordinateDrive::TCoordinate2D &aCoordinateTo,
                   MaCI::CoordinateDrive::TCoordinateArray &OUT,
                   unsigned int aTimeout_ms);

  bool GetPlan(const MaCI::CoordinateDrive::TCoordinate2D &aCoordinateFrom,
                   const MaCI::CoordinateDrive::TCoordinate2D &aCoordinateTo,
                   MaCI::CoordinateDrive::TCoordinateContainer &OUT,
                   unsigned int aTimeout_ms);
                   
      /** 
       * Set coordinateDrive driving speed
       * 
       * This function sets driving speed 
       *
       * @param[in] aSpeed            Speed to set
       * 
       * @return                      True if succeeded, false if failed(timeout, no connection..)
       */
      bool SetDrivingSpeed(const MaCI::CoordinateDrive::TSpeed aSpeed);

      /**
       * Gets the maximum driving speed to set
       *
       * @param[out] aMaxDrivingSpeed Maximum driving speeds are stored here
       *
       * @return                      True if succeeded, false if failed(timeout, no connection..)
       */
      bool GetMaximumDrivingSpeed(MaCI::CoordinateDrive::TSpeed &OUT,
                                  unsigned int aTimeout_ms);


      // Derived from MaCI::CMaCI
	    MaCI::EMaCIError Open();
	    MaCI::EMaCIError Close();    
	  //  void SetMaCICtrlServer(MaCI::MaCICtrl::CMaCICtrlServer *aMaCICtrlServer);
	    void SetDefaultTarget(const std::string &aDefaultTarget);
	    void SetStaticLinkDelay(const unsigned int aDelay_ms);
	    bool SetGroupName(const std::string &aGroupName);	      
		bool SetDefaultTarget(const MaCI::MaCICtrl::SMaCISL &aMaCISL, const unsigned int aTimeout_ms = 2000);

    };
  }
}
